#include <buola/buola.h>

#if defined BUOLA_HAVE_OPENNI && !defined BUOLA_HAVE_OPENNI2

#include "ckinectdevice_openni.h"
#include "ckinectdriver_openni.h"

#include <buola/image/format.h>
#include <buola/image/io.h>

namespace buola { namespace cvi {

CKinectDevice::CKinectDevice(xn::Context &pXNContext,const xn::NodeInfo &pNodeInfo)
    :   mXNContext(pXNContext)
    ,   mDeviceInfo(pNodeInfo)
    ,   mConnectionString(mDeviceInfo.GetCreationInfo())
    ,   mInitialized(false)
    ,   mSyncIndex(0)
    ,   mStartCount(0)
    ,   mStarted(false)
{
    msg_info() << pNodeInfo.GetDescription().strName << "\n";
    msg_info() << pNodeInfo.GetDescription().strVendor << "\n";
    msg_info() << pNodeInfo.GetDescription().Type << "\n";
    msg_info() << pNodeInfo.GetCreationInfo() << "\n";
}

CKinectDevice::~CKinectDevice()
{
    
}

void CKinectDevice::Start()
{
    if(!mStartCount++)
    {
        lock_guard<mutex> lLock(mMutex);
        if(mThread.get_id()==thread::id())
            mThread=thread([this](){OnThread();});
        mCondVar.notify_one();
    }
}

void CKinectDevice::Stop()
{
    mStartCount--;
}

std::pair<img::CImage_rgb8,img::CImage_gray32f> CKinectDevice::GetOne()
{
    std::pair<img::CImage_rgb8,img::CImage_gray32f> lResult;
    Start();
    {
        unique_lock<mutex> lLock(mSyncMutex);
        int lSyncIndex=mSyncIndex;
        while(lSyncIndex==mSyncIndex)
            mSyncCondVar.wait(lLock);
        lResult.first=share(mSyncImageRGB);
        lResult.second=share(mSyncImageDepth);
    }
    Stop();
    
    return lResult;
}


bool CKinectDevice::HasDepthStream()
{
    if(!mInitialized)
        Initialize();

    return mDepthGenerator.IsValid();
}

bool CKinectDevice::HasImageStream()
{
    if(!mInitialized) 
        Initialize();

    return mImageGenerator.IsValid();
}

bool CKinectDevice::HasIRStream()
{
    if(!mInitialized) 
        Initialize();

    return mIRGenerator.IsValid();
}

uint64_t CKinectDevice::GetIntProperty(const std::string &pName)
{
    XnUInt64 lValue;
    if(mDepthGenerator.GetIntProperty(pName.c_str(),lValue)!=XN_STATUS_OK)
        throw std::runtime_error("CKinectDevice: can't read integer property \""+pName+"\"");        
    return lValue;
}

double CKinectDevice::GetDoubleProperty(const std::string &pName)
{
    XnDouble lValue;
    if(mDepthGenerator.GetRealProperty(pName.c_str(),lValue)!=XN_STATUS_OK)
        throw std::runtime_error("CKinectDevice: can't read double property \""+pName+"\"");        
    return lValue;
}

void CKinectDevice::SetIntProperty(const std::string &pName,uint64_t pValue)
{
    if(mDepthGenerator.SetIntProperty(pName.c_str(),pValue)!=XN_STATUS_OK)
        throw std::runtime_error("CKinectDevice: can't set integer property \""+pName+"\"");        
}

void CKinectDevice::SetDoubleProperty(const std::string &pName,double pValue)
{
    if(mDepthGenerator.SetRealProperty(pName.c_str(),pValue)!=XN_STATUS_OK)
        throw std::runtime_error("CKinectDevice: can't set double property \""+pName+"\"");        
}


void CKinectDevice::Initialize()
{
    lock_guard<mutex> lLock(mMutex);
    if(mInitialized) return;
    
    XnStatus lStatus;

    if((lStatus=mXNContext.CreateProductionTree(mDeviceInfo,mXNDevice))!=XN_STATUS_OK)
        throw std::runtime_error("can't create XN device: "+std::string(xnGetStatusString(lStatus)));
    
    xn::Query lQuery;
    lQuery.AddNeededNode(mDeviceInfo.GetInstanceName());
    
    if((lStatus=mDepthGenerator.Create(mXNContext,&lQuery))!=XN_STATUS_OK)
        throw std::runtime_error("creating depth generator failed"+std::string(xnGetStatusString(lStatus)));
    if((lStatus=mImageGenerator.Create(mXNContext,&lQuery))!=XN_STATUS_OK)
        throw std::runtime_error("creating image generator failed"+std::string(xnGetStatusString(lStatus)));
    if((lStatus=mIRGenerator.Create(mXNContext,&lQuery))!=XN_STATUS_OK)
        throw std::runtime_error("creating IR generator failed"+std::string(xnGetStatusString(lStatus)));
    
    mDepthFocalLengthSXGA=0;
    //This magic value is taken from a calibration routine.
    mRGBFocalLengthSXGA=1050;

    if(mDepthGenerator.IsValid())
    {
        double lPixelSize=GetDoubleProperty("ZPPS");
        uint64_t lDepthFocalLengthSXGA=GetIntProperty("ZPD");
        mBaseline=0.01*GetDoubleProperty("LDDIS"); //in meters
        mShadowValue=GetIntProperty("ShadowValue");
        mNoSampleValue=GetIntProperty("NoSampleValue");

        msg_info() << "getting depth generator properties\n";
        msg_info() << "RegistrationType:" << GetIntProperty("RegistrationType") << "\n";
//        SetIntProperty("RegistrationType",2);
        //convert to meters

        //focal length from mm to pixels
        mDepthFocalLengthSXGA=float(lDepthFocalLengthSXGA/lPixelSize);
        
        msg_info() << mDepthGenerator.GetAlternativeViewPointCap().IsViewPointAs(mImageGenerator) << "\n";
        mDepthGenerator.GetAlternativeViewPointCap().SetViewPoint(mImageGenerator);
    }

    mInitialized=true;
}

void CKinectDevice::OnThread()
{
    msg_info() << "onthread\n";
    try
    {
        int lFrameCount=0;
        
        while(true)
        {
            if(!mStartCount)
            {
                lFrameCount=0;
                
                unique_lock<mutex> lLock(mMutex);
                
                while(!mStartCount)
                {
                    if(mStarted)
                        DoStop();
                    
                    mCondVar.wait(lLock);
                }
            }
            
            if(!mStarted)
            {
                DoStart();
            }
            
            DoIteration(++lFrameCount);
        }
    }
    catch(std::exception &pE)
    {
        msg_info() << "got exception in thread " << pE.what() << "\n";
    }
}

void CKinectDevice::DoStart()
{
    if(!HasDepthStream())
        throw XInvalid("device doesn't have a depth stream");

    if(!mDepthGenerator.IsGenerating())
    {
        if(mDepthGenerator.StartGenerating()!=XN_STATUS_OK)
            throw std::runtime_error("can't start depth stream");
    }

    if(HasImageStream())
    {
        if(!mImageGenerator.IsGenerating())
        {
            if(mImageGenerator.StartGenerating()!=XN_STATUS_OK)
                throw std::runtime_error("can't start image stream");
        }
    }
    
    mStarted=true;
}

void CKinectDevice::DoStop()
{
    
}

void CKinectDevice::DoIteration(int pCount)
{
    xn::DepthMetaData lDepthMD;
    mDepthGenerator.WaitAndUpdateData();
    mDepthGenerator.GetMetaData(lDepthMD);
    const XnDepthPixel *lDepthSrc=lDepthMD.Data();
    xn::ImageMetaData lImageMD;
    mImageGenerator.WaitAndUpdateData();
    mImageGenerator.GetMetaData(lImageMD);
    const uint8_t *lRGBSrc=lImageMD.Data();
    
    CSize_i lDepthSize(lDepthMD.XRes(),lDepthMD.YRes());
    CSize_i lRGBSize(lImageMD.XRes(),lImageMD.YRes());

    if(lDepthSize!=lRGBSize)
        throw XInvalid("CKinectDevice: image and depth sizes don't match");

    img::CImage_gray32f lDepth(lDepthSize);
    img::CImage_rgb8 lRGB(lRGBSize);
    
    auto lDepthDst=lDepth.RawData();
    auto lRGBDst=lRGB.RawData();

    for(int y=0;y<lRGBSize.y;y++)
    {
        for(int x=0;x<lRGBSize.x;x++)
        {
            auto lDepthValue=*lDepthSrc++;
            if(lDepthValue==0)
            {
                *lDepthDst=NAN;
            }
            else
            {
                *lDepthDst=lDepthValue/1000.0;
            }
            
            *lRGBDst++=*lRGBSrc++;
            *lRGBDst++=*lRGBSrc++;
            *lRGBDst++=*lRGBSrc++;
        }
    }
    
    lRGB.TimeStampNow();
    
    sImages(lRGB,lDepth);

    if(pCount>10)
    {
        lock_guard<mutex> lLock(mSyncMutex);
    
        mSyncIndex++;
        mSyncImageRGB=std::move(lRGB);
        mSyncImageDepth=std::move(lDepth);
        mSyncCondVar.notify_all();
    } 
    else
    {
        msg_info() << "ignoring one\n";
    }
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
